
#include <iostream>
#include <fstream>

#include <opencv2/opencv.hpp>
#include "TrackerKCF.hpp"

#include <random>

#include "utils.hpp"

using namespace std;
using namespace cv;
using namespace handtrack;
//[510,460,96,96]
void test_cpu_main(int x, int y, int w, int h, int patch_size, const char *in_path, const char *out_path)
{
    Mat frame;
    Rect curr_res;
    Rect prev_res;
    int key;
    // Rect ROI(520, 470, 128, 128);
    Rect ROI(x, y, w, h);


    char img_file_name[1024] = {0};
    char out_img_file_name[1024] = {0};
    int frame_cnt = 0;
    sprintf(img_file_name, "%s/%04d.png", in_path, frame_cnt);
    frame = imread(img_file_name, IMREAD_GRAYSCALE);
    Size S = Size((int)frame.cols, (int)frame.rows);
    sprintf(out_img_file_name, "%s/log.txt", out_path);
    ofstream log(out_img_file_name, ios::out|ios::trunc);
    random_device rd;  // will be used to obtain a seed for the random number engine
    mt19937 gen(rd()); // standard mersenne_twister_engine seeded with rd()
    uniform_int_distribution<> distrib(-48, 48);

    TrackerKCF::Ptr tracker = TrackerKCF::Ptr(
        new TrackerKCF(patch_size, false)
    );
    curr_res = ROI;
    tracker->update(curr_res, frame, 1.f);
    prev_res = curr_res;
    int offset_x = 0;//distrib(gen);
    int offset_y = 0;//distrib(gen);
    prev_res.x += offset_x;
    prev_res.y += offset_y;
    cout << "offset: " << offset_x << ", " << offset_y << endl;
    while (frame_cnt < 0) {
        sprintf(img_file_name, "%s/%04d.png", in_path, frame_cnt);
        frame = imread(img_file_name);
        if (frame.empty()) {
            break;
        }
        Mat gray;
        cvtColor(frame, gray, COLOR_BGR2GRAY);
        TicToc tic;
        float curr_peak_value = 0.f;
        curr_res = tracker->detect(prev_res, gray, curr_peak_value);
        uint64_t detect_cost = tic.toc();
        tic.tic();
        // if (curr_peak_value < 0.4f) break;
        tracker->update(curr_res, gray, 0.01f);
        uint64_t update_cost = tic.toc();
        cout << frame_cnt << " peak_value: " << curr_peak_value << ", detect: " << detect_cost << "us, update: " << update_cost << "us"  << endl;
        rectangle(
            frame,
            Point(prev_res.x, prev_res.y),
            Point(prev_res.x + prev_res.width, prev_res.y + prev_res.height),
            Scalar(255, 0, 0), 2, 8
        );
        prev_res = curr_res;
        offset_x = distrib(gen);
        offset_y = distrib(gen);
        prev_res.x += offset_x;
        prev_res.y += offset_y;
        cout << "offset: " << offset_x << ", " << offset_y << endl;
        rectangle(
            frame,
            Point(curr_res.x, curr_res.y),
            Point(curr_res.x + curr_res.width, curr_res.y + curr_res.height),
            Scalar(0, 255, 0), 2, 8
        );
        #if 0
        sprintf(out_img_file_name, "%s/%04d.png", out_path, frame_cnt);
        // cout << out_img_file_name << endl;
        imwrite(out_img_file_name, frame);
        #endif

        log << detect_cost << ", " << update_cost << endl;
        frame_cnt++;
    }

}
